//
// Created by whitby on 8/22/23.
//

#include "Camera.h"
#include "Utils/Converter.h"

#include <iostream>

using namespace std;

namespace mono_orb_slam2 {
    Camera *Camera::camera_ptr = nullptr;

    bool Camera::create(const cv::FileNode &cameraNode) {
        if (camera_ptr == nullptr) {
            int width, height, fps;
            cv::Mat K;
            cameraNode["Width"] >> width, cameraNode["Height"] >> height;
            cameraNode["fps"] >> fps;
            cameraNode["CameraMatrix"] >> K;
            Eigen::Matrix3f eigenK = Converter::toMatrix3f(K);
            camera_ptr = new Camera(width, height, fps, eigenK);
            return true;
        } else {
            cerr << "camera_ptr not null" << endl;
            return false;
        }
    }

    void Camera::destroy() {
        delete camera_ptr;
        camera_ptr = nullptr;
    }

    const Camera *Camera::getCamera() {
        return camera_ptr;
    }

    void Camera::print() const {
        cout << endl << "Camera Parameters" << endl;
        cout << " -image size: " << width << " x " << height << endl;
        cout << " -fps: " << fps << endl;
        cout << " -calibrate parameters: " << fx << ", " << fy << ", "
             << cx << ", " << cy << endl << endl;
    }

    Eigen::Vector2d Camera::project(const Eigen::Vector3d &P) const {
        const double x = P[0] / P[2], y = P[1] / P[2];
        double u = fx * x + cx, v = fy * y + cy;
        return {u, v};
    }

    Eigen::Vector2f Camera::project(const Eigen::Vector3f &P) const {
        const float x = P[0] / P[2], y = P[1] / P[2];
        float u = fx * x + cx, v = fy * y + cy;
        return {u, v};
    }

    bool Camera::isInImage(const Eigen::Vector2f &p) const {
        if (p[0] < 0 || p[0] >= width || p[1] < 0 || p[1] >= height) return false;
        return true;
    }

    Eigen::Matrix<double, 2, 3> Camera::getProjJacobian(const Eigen::Vector3d &Pc) const {
        Eigen::Matrix<double, 2, 3> projectJacobian;
        projectJacobian << fx / Pc[2], 0, -fx * Pc[0] / (Pc[2] * Pc[2]), 0, fy / Pc[2], -fy * Pc[1] / (Pc[2] * Pc[2]);
        return projectJacobian;
    }

    Eigen::Vector3f Camera::pixelToNormalizePlane(const cv::Point2f &pt) const {
        return {(pt.x - cx) * inv_fx, (pt.y - cy) * inv_fy, 1.f};
    }
} // mono_orb_slam2